/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include<stdlib.h>
#include<stdio.h>
#include <string>
#include <map>
#include <vector>
#include <list>
#include <sstream>
#include "RangingServer.hpp"
#include "PositionServer.hpp"
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"

/// Converts one position and one ranging logfile into 3D points and saves data to "data.txt"
///
//****************************************************************************************
//****************************************************************************************
/// Logfile related variables
//****************************************************************************************
//****************************************************************************************
///For Ranging data
std::map<gim::time,MaCI::Ranging::CRangingData *> rData; 		///< map of mapdata
static float RangingMinDistance = 0;
static float RangingMaxDistance = 80;
static float lx=0,ly=0,lz=0,la=0; ///< laser position
std::vector<gim::time> rTimes;
gim::time rBegin;
gim::time rEnd;
///For Position Data
std::map<gim::time,MaCI::Position::CPositionData *> pData; ///< map of positiondata
std::vector<gim::time> pTimes;
gim::time pBegin;
gim::time pEnd;

/**
* Read the Ranging log file and stores the values into stl::map
*/
bool readRangingFile(std::string aFileName){
	FILE *iFile;
	
	dPrint(3,"Opening file '%s'",aFileName.c_str());
	iFile= fopen(aFileName.c_str(),"rt");
	char c[200];
	char interfaceName[64];
	std::string temp;
	//Read the four first lines..and get the interfacename...
	fgets (c, 200,iFile );
	
	fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
	//iName = "MaCI_Ranging."+std::string(interfaceName);

	fgets (c, 200,iFile );
	fgets (c, 200,iFile );
	fgets (c, 200,iFile );
	dPrint(1,"%s",c);
	
	std::string additionalInformation(c);
	//Search for device limits
	std::string::size_type loc = additionalInformation.find( "maxDistance:", 0 );
	std::istringstream stream;
	std::string s(c);
	stream.str(s);
	
	if( loc != std::string::npos ) {	
		float maxDistance,minDistance;
		stream >> temp >>maxDistance >>temp >>temp>> minDistance;
		RangingMaxDistance = maxDistance;
		RangingMinDistance = minDistance;
	}
	//Search for device position
	loc = additionalInformation.find( "positionX", 0 );
	if( loc != std::string::npos ) {
		float x=1,y=1.1,z=1.1,heading=1.1;
		stream >>temp>>x>>temp>>temp>>y>>temp>>temp>>z>>temp >>temp >>heading;
		dPrint(10,"got device position (%f, %f, %f, %f)",x,y,z,heading);
		lx = x; ly=y; lz=z; la = heading;
	}
	
	using namespace MaCI::Ranging;
	MaCI::Common::TTimestamp timestamp;
	float beam_width = 0.0,beam_count = 0.0;
	std::vector<float> angles;
	std::vector<float> distances;
	float angle = 0.0,distance = 0.0;
	int number = 1;
	bool first = true;
	while(number != EOF){
		
		angles.clear();
		distances.clear();
		number = 1;
		number = fscanf(iFile,"%u %u %f %f",
										&(timestamp.timestamp_s),
										&(timestamp.timestamp_us),
										&beam_width,
										&beam_count);

		if(first && number == 4){
			rBegin = timestamp.GetGimTime();
			first = false;
		}
		//    dPrint(1,"timestamp %s, number %d",timestamp.GetGimTime().toString().c_str(),number);

		for(unsigned j = 0; j< beam_count;j++){
		number = fscanf(iFile,"%f %f",
					&angle,
					&distance);
					
					angles.push_back(angle);
					distances.push_back(distance);
					
		}

		if(angles.size() >0){
			MaCI::Ranging::CRangingData *rdata = new MaCI::Ranging::CRangingData();
			rdata->CreateInternalBinBag();
			rdata->SetTimestamp(timestamp);

			rdata->SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beam_width,beam_count));
			
			for(unsigned int i = 0; i< angles.size(); i++){
				rdata->AddDistance(MaCI::Ranging::TDistance(angles[i], distances[i]));
			}

			rData[timestamp.GetGimTime()] = rdata;
			rTimes.push_back(timestamp.GetGimTime());

		}
	}
	if(!first){
		rEnd = timestamp.GetGimTime();
	}
	fclose(iFile);
	return true;
}

/**
* Read the Position log file and stores the values into stl::map
*/
bool readPositionFile(std::string aFileName){
	FILE *iFile;
	bool iContainsTimestamp=false;
	bool iContainsPosition=false;
	bool iContainsVariance=false;
	bool iContainsProbability=false;
	bool iContainsSpeed=false;
	dPrint(3,"Opening file '%s'",aFileName.c_str());
	iFile= fopen(aFileName.c_str(),"rt");
	
	char c[200];
	char interfaceName[64];
	//Read the four first lines..and get the interfacename...
	fgets (c, 200,iFile );
	
	fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
	fgets (c, 200,iFile );
	fgets (c, 200,iFile );
	//Look what datatypes are logged
	std::string types(c);
	std::string finalPrint = "Log file contains : ";
	std::string::size_type loc = types.find( "timestamp_sec", 0 );
	if( loc != std::string::npos ) {
		iContainsTimestamp=  true;
		finalPrint +="'timestamp' ";
	}
	
	loc = types.find("position_x",0);
	if( loc != std::string::npos ) {
		iContainsPosition=  true;
		finalPrint +="'position' ";
	}
	
	
	loc = types.find("variance_x",0);
	if( loc != std::string::npos ) {
		iContainsVariance=  true;
		finalPrint +="'variance' ";
	}
	
	
	loc = types.find("probability",0);
	if( loc != std::string::npos ) {
		
		iContainsProbability=  true;
		finalPrint += "'probability' ";
	}
	dPrint(1,"%s",c);
	
	loc = types.find("speed",0);
	if( loc != std::string::npos ) {
		
		iContainsSpeed=  true;
		finalPrint += "'speed' ";
	}
	finalPrint += "fields.";
	
	dPrint(5,"%s",finalPrint.c_str());
	
	using namespace MaCI::Position;
	MaCI::Common::TTimestamp timestamp;
	float posX,posY,posA,varX,varY,varA,prob,speed,angularSpeed;
	int number = 1;
	bool first = true;
	if(!iContainsTimestamp){
		dPrint(3,"File doesn't contain timestamps. can't play data without timestamp");
		return false;
	}
	while(number != 0 && number != EOF){
		
		number = fscanf(iFile,"%u %u",
										&(timestamp.timestamp_s),
										&(timestamp.timestamp_us));
		if(first && number == 2){
			pBegin = timestamp.GetGimTime();
			first = false;
		}    
		
		if(iContainsPosition){
			number = fscanf(iFile,"%f %f %f",
											&posX,&posY,&posA);
											
											if(number != 3){
												
												continue;
											}
		}
		if(iContainsVariance){
			number = fscanf(iFile,"%f %f %f",
											&varX,&varY,&varA);
											if(number != 3){
												
												continue;
											}
		}
		if(iContainsProbability){
			number = fscanf(iFile,"%f",
											&prob);
											if(number != 1){
												
												continue;
											}
		}
		
		if(iContainsSpeed){
			number = fscanf(iFile,"%f %f",
											&speed,&angularSpeed);
											if(number != 2){
												
												continue;
											}
											
		}
		if(number> 0){
			CPositionData *pos = new CPositionData();
			pos->CreateInternalBinBag();
			pos->SetTimestamp(timestamp);
			if(iContainsPosition){
				pos->SetPose2D(TPose2D(posX,posY,posA));
			}
			if(iContainsVariance){
				pos->SetVariance2D(TVariance2D(varX,varY,varA));
			}
			
			if(iContainsProbability){
				pos->SetProbability(TProbability(prob));
			}
			if(iContainsSpeed){
				pos->SetSpeed2D(TSpeed2D(speed,angularSpeed));
			}
			//pos->Print(1);
			pData[timestamp.GetGimTime()] = pos;
			pTimes.push_back(timestamp.GetGimTime());
		}
		}
		
		if(!first){
			pEnd = timestamp.GetGimTime();
		}
		if(number == EOF){
			return true;
		}else{
			return false;
		}
	return true;
}
void showUsage(){
	fprintf(stderr,"tilted laser data manipulator\n");
	fprintf(stderr,"./tilted_laser -P [positionLogfile] -R [rangingLogFile]\n");
}

void writeScanLine(FILE *f,MaCI::Ranging::CRangingData *r,MaCI::Position::CPositionData *p){
	const MaCI::Position::TPose2D *pose; ///< Pose
	const MaCI::Ranging::TDistance *r_meas; ///Laser Measurement
	pose = p->GetPose2D();
	
	if(pose == NULL){
		fprintf(stderr,"FAILED TO PARSE!!\n");
		exit(1);
	
	}
	
	fprintf(f,"%f %f %f ",pose->x,pose->y,pose->a); ///< Pose
	int i;
	for(i=0;i<r->GetDistanceElementCount()-1;i++){
		r_meas = r->GetDistance(i);
		fprintf(f,"%f %f ",r_meas->distance,r_meas->angle);
	}
	fprintf(f,"%f %f\n",r_meas->distance,r_meas->angle);
}


/********************************************************************************/
/********************************************************************************/
///Execute
/********************************************************************************/
/********************************************************************************/
void execute(){
	int rangingCounter=0;
	int positionCounter = 0;
	
	std::map<gim::time,MaCI::Ranging::CRangingData *>::iterator iter=rData.find(rTimes[rangingCounter]);
	FILE *fout = fopen("measurement-out.txt","wt");
	while(rangingCounter<rTimes.size()-1){
		
		rangingCounter++;
		iter=rData.find(rTimes[rangingCounter]);
		
		gim::time diff = rTimes[rangingCounter]-pTimes[positionCounter];
		
		
		while(1){ ///search correct pose	
			gim::time diff2 = rTimes[rangingCounter]-pTimes[positionCounter+1];
			
			if(fabs(diff2.getTimeInSeconds()) <= fabs(diff.getTimeInSeconds())){
				positionCounter++;
				diff = diff2;
			}else{ ///This is the first that is bigger
				break;
			}
		}
		diff = rTimes[rangingCounter]-pTimes[positionCounter];
		dPrint(10,"(%d, %d) %s %s Timediff = (%lf)",rangingCounter,positionCounter,
					 rTimes[rangingCounter].toString().c_str(),
					 pTimes[positionCounter].toString().c_str(),
					 diff.getTimeInSeconds() );
		
		std::map<gim::time,MaCI::Position::CPositionData *>::iterator piter=pData.find(pTimes[positionCounter]);
		writeScanLine(fout,iter->second,piter->second);
	}
	
	
	fclose(fout);

}







/********************************************************************************/
/********************************************************************************/
/// Main
/********************************************************************************/
/********************************************************************************/

int main(int argc, char *argv[]){
	// Init Debug lib
	int verbose = 5;
	debugInit();
	debugSetGlobalDebugLvl(verbose);
	debugSetLogFilename("tilted_laser.log");
	std::string positionLogFile;
	std::string rangingLogFile;
	
	
	bool parse = true;
	while(parse) {
		int c;
		c = getopt(argc, argv, "P:R:hqv");
		switch(c) {
			
			/////////////////////////////////
			////// Application specific /////
			/////////////////////////////////
			case 'P': {
					positionLogFile = optarg;
				break;
			}
			case 'R': {
				rangingLogFile = optarg;
				break;
			}
			
				//////////////////////////////////
				//// GENERIC:                 ////
				//////////////////////////////////
			case 'v':
				// Increase only if not previously set to 0 (by 'quiet')
				if (verbose > 0)
					debugSetGlobalDebugLvl(++verbose);
				break;
				
			case 'q':
				verbose = 0;
				debugSetGlobalDebugLvl(0);
				break;
				
			case 'h':
			case '?':
				showUsage();
				exit(1);
				break;
				
			case -1:
				parse = false;
				break;
		}
	}
	
	readPositionFile(positionLogFile);
	readRangingFile(rangingLogFile);
	
	execute();
	
	
	
	return 0;
}
